import rospy
import json
import os
from geometry_msgs.msg import PoseStamped
from math import *
from move_base_msgs.msg import MoveBaseActionGoal
import tf


class NavTarget():
    def __init__(self, config_file = '.NavGoalList.json', dir = './'):
        rospy.init_node("rei_web_node",disable_signals=True,anonymous=True)
        self._config_file = config_file
        self.dir = dir
        self.NavTarget_Goals = dict()
        self.load()
        
    def load(self):
        if os.path.exists(self.dir):
            path = os.path.join(self.dir,self._config_file)
        if not os.path.exists(path):
            rospy.logerr("path not found")
        else:
            with open(path, 'r', encoding='utf-8') as f:
                self.NavTarget_Goals = json.load(f)
            print(self.NavTarget_Goals)
    
    def get_2D_NavGoal(self,name):
        """
        获取/move_base_simple/goal消息的pose
        :param name: 为pose命名
        """
        msg = rospy.wait_for_message('/move_base_simple/goal',PoseStamped)
        pose = {
            "name": name,
            "pose":{
                "position":{
                    "x":msg.pose.position.x,
                    "y":msg.pose.position.y,
                    "z":msg.pose.position.z,
                },
                "orientation":{
                    "x":msg.pose.orientation.x,
                    "y":msg.pose.orientation.y,
                    "z":msg.pose.orientation.z,
                    "w":msg.pose.orientation.w,
                },
            },
        }
        self.NavTarget_Goals[name] = pose
        self.save()

    def get_NowPose(self,name):
        """
        获取map与base_footprint的tf关系pose
        :param name: 为pose命名
        """
        listener = tf.TransformListener()
        try:
            listener.waitForTransform('/map','/base_footprint',rospy.Time(0.0),rospy.Duration(5.0))
            (trans, rot) = listener.lookupTransform('/map','/base_footprint',rospy.Time(0))
            pose = {
                "name": name,
                "pose":{
                    "position":{
                        "x":trans[0],
                        "y":trans[1],
                        "z":trans[2],
                    },
                    "orientation":{
                        "x":rot[0],
                        "y":rot[1],
                        "z":rot[2],
                        "w":rot[3],
                    },
                },
            }
            self.NavTarget_Goals[name] = pose
            self.save()
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as err:
            print("[ERROR]",err)

    def set_NavTarget(self,name,x=None,y=None,angle=None):
        """
        设置一个导航目标点的pose
        :param name: 为pose命名
        :param x: x
        :param y: y
        :param angle: z轴旋转角度
        """
        z = angle*pi/180
        pose = {
            "name": name,
            "pose":{
                "position":{
                    "x":x,
                    "y":y,
                    "z":0,
                },
                "orientation":{
                    "x":0,
                    "y":0,
                    "z":sin(z/2),
                    "w":cos(z/2),
                },
            },
        }
        self.NavTarget_Goals[name] = pose
        self.save()

    def GoalMsg(self,name,frame_id="map"):
        """
        输入导航点名称 获取actiongoal消息
        :param name: 目标名称
        :param frame_id: 坐标系
        """
        try:
            pose = self.NavTarget_Goals[str(name)]
        except KeyError:
            print("未查到导航点")
        target = MoveBaseActionGoal()
        target.goal.target_pose.header.frame_id = frame_id # 使用map的frame定义goal的frame id
        target.goal.target_pose.header.stamp = rospy.Time.now() # 设置时间戳  
        # 设置导航点位置
        target.goal.target_pose.pose.position.x = pose["pose"]["position"]["x"]
        target.goal.target_pose.pose.position.y = pose["pose"]["position"]["y"]
        target.goal.target_pose.pose.position.z = pose["pose"]["position"]["z"]
        target.goal.target_pose.pose.orientation.x = pose["pose"]["orientation"]["x"]
        target.goal.target_pose.pose.orientation.y = pose["pose"]["orientation"]["y"]
        target.goal.target_pose.pose.orientation.z = pose["pose"]["orientation"]["z"]
        target.goal.target_pose.pose.orientation.w = pose["pose"]["orientation"]["w"]
        return target.goal

    def delete(self,name):
        self.NavTarget_Goals.pop(str(name))
        self.save()

    def delete_all(self):
        self.NavTarget_Goals = {}
        self.save()

    def save(self):
        """
        保存导航目标pose列表
        """
        path = os.path.join(self.dir,self._config_file)
        if not os.path.exists(self.dir):
            os.makedirs(self.dir)
        if not os.path.exists(path):
            os.mknod(path)  
        with open(path,'w+') as f:
            json.dump(self.NavTarget_Goals,f)
        
        
        